#include "s_motor_control.h"
#include "h_stm32_hal_defines.h"

#include <stdio.h>
#include <string.h>


#define MOTOR_PWM_TIME     htim3

extern TIM_HandleTypeDef MOTOR_PWM_TIME;

////////////////////////////////////////////////////////////////////////////////

#define s_motor0_pwm_start()               \
    HAL_TIM_PWM_Start(&MOTOR_PWM_TIME, TIM_CHANNEL_2)
#define s_motor0_pwm_stop()                \
    HAL_TIM_PWM_Stop(&MOTOR_PWM_TIME, TIM_CHANNEL_2)

#define s_motor1_pwm_start()               \
    HAL_TIM_PWM_Start(&MOTOR_PWM_TIME, TIM_CHANNEL_3)
#define s_motor1_pwm_stop()                \
    HAL_TIM_PWM_Stop(&MOTOR_PWM_TIME, TIM_CHANNEL_3)

static void s_motor_change_frq(unsigned int frq)
{
    unsigned int new_main_fep_divide = 0U;

    new_main_fep_divide = (1024000.0 / frq);

    /* Set the Autoreload value */
    MOTOR_PWM_TIME.Instance->ARR = new_main_fep_divide;

    /* Set the Capture Compare Register value */
    MOTOR_PWM_TIME.Instance->CCR1 = new_main_fep_divide / 2;
}

// 0 - 100
void s_motor0_pwm_set_pulse(unsigned int value)
{  
	value = (MOTOR_PWM_TIME.Instance->ARR) * value / 100.0;
    MOTOR_PWM_TIME.Instance->CCR2 = value;
}
void s_motor1_pwm_set_pulse(unsigned int value)
{  
	value = (MOTOR_PWM_TIME.Instance->ARR) * value / 100.0;
    MOTOR_PWM_TIME.Instance->CCR3 = value;
}
/* USER CODE END 4 */
////////////////////////////////////////////////////////////////////////////////
static unsigned int s_motor0_value = 0U;

signed char s_motor_control_init(void)
{
	
	s_motor0_pwm_set_pulse(s_motor0_value);
	
	s_motor1_pwm_set_pulse(0);
	s_motor0_pwm_start();
	s_motor1_pwm_start();

	return 0;
}

signed char s_motor_control_setPower(unsigned char pw)
{
	if(pw > 100) {
		pw = 100;
	}
	s_motor0_pwm_set_pulse(pw);
	return 0;
}

signed char s_motor_control_start(void)
{
	s_motor0_pwm_start();
	return 0;
}

signed char s_motor_control_stop(void)
{
	s_motor0_pwm_stop();
	return 0;
}

signed char s_motor0_change(void)
{
	if(s_motor0_value >= 100) {
		s_motor0_value = 0;
	} else {
		s_motor0_value += 10;
	}
	s_motor0_pwm_set_pulse(s_motor0_value);
	return 0;
}
